#include "slep.h"
#include "hr_time.h"
#include "epph.h"
#include "Logistic.h"



FUNVAL nnLogisticC(double* A, double* y, double z, OPTS opt){
	int m,n,iterStep,i, bFlag, steps;
	double m1, rsL2,m2, sc, l_sum, r_sum, fun_x, fun_s, gc, c, cp, gamma, ccp ,  alpha, fVal, fValp, lambda0, root, tp,t;
	FUNVAL funret;
	double *weight,*weighty, *aa,*bb, *x, *xp, *xxp, *Ax, *Axp, *funVal, *ValueL, *s, *v, *g, *prob, *As, *b, *vv, *xx;

	m= opt.m;
	n= opt.n;

	b = (double *) malloc( sizeof(double) * n);
	weight = (double *) malloc( sizeof(double) * m);
	x = (double *) malloc( sizeof(double) * n);
	Ax = (double *) malloc( sizeof(double) * m);
	weighty = (double *) malloc( sizeof(double) * m);
	xp = (double *) malloc( sizeof(double) * n);
	Axp = (double *) malloc( sizeof(double) * m);
	xxp = (double *) malloc( sizeof(double) * n);
	s = (double *) malloc( sizeof(double) * n);
	As = (double *) malloc( sizeof(double) * m);
	aa = (double *) malloc( sizeof(double) * m);
	bb = (double *) malloc( sizeof(double) * m);
	prob = (double *) malloc( sizeof(double) * m);
	g = (double *) malloc( sizeof(double) * n);
	v = (double *) malloc( sizeof(double) * n);
	vv = (double *) malloc( sizeof(double) * n);
	xx = (double *) malloc( sizeof(double) * n);
	funVal = (double *) malloc( sizeof(double) * opt.maxIter);
	ValueL = (double *) malloc( sizeof(double) * opt.maxIter);

	initNormalization(&opt,A);

	m1 = normalizeWeights(&opt,weight,y,b);
	m2 = 1.0 - m1;

	rsL2 = opt.rsL2;

	if( opt.init == 2){
		memset(x,0,sizeof(double) * n);
		c = 0;
	}
	else{
		if(hasGot(&opt,"x0"))
			dcopy(n,opt.x0,1,x,1);
		else
			memset(x,0,sizeof(double) *n);

		if(hasGot(&opt,"c0"))
			c = opt.c0;
		else
			c=0;
	}

	normalizedmv('N',&opt,A,x,Ax);

	// The main program

	bFlag = 0;

	gamma = 1.0/m + rsL2;


	// weighty = diag(weight) * y;
	dsbmv('U',m,0,1.0,weight,1,y,1,0.0, weighty,1);

	// xp = x; Axp = Ax; xxp = zeros
	// cp = c; ccp = 0;
	dcopy(n,x,1,xp,1);
	dcopy(m,Ax,1,Axp,1);
	memset(xxp,0,sizeof(double) * n);
	cp = c;
	ccp = 0.0;

	// The Armijo Goldstein line search schemes + accelerated gradient scheme
	tp = 0.0;
	t = 1.0;

	lambda0 = 0.0;

	for( iterStep = 0 ; iterStep < opt.maxIter ; iterStep++){
		alpha = (tp - 1)/t;
		// s = x + alpha * xxp; 
		dcopy(n,x,1,s,1);
		daxpy(n,1.0,xxp,1,s,1);

		sc = c + alpha * ccp;

		// As = Ax + alpha * ( Ax - Axp);
		dcopy(m,Ax,1,As,1);
		dscal(m,1.0 + alpha, Ax,1);
		daxpy(m,-1.0,Axp,1,As,1);

		// aa = - diag(y) * ( A*s + sc);
		dcopy(m,As,1,aa,1);
		daxpy(m,sc,oneVector,1,aa,1);
		dtbmv('U','N','N',m,0,y,1,aa,1);
		dscal(n,-1.0,aa,1);


		fun_s = 0.0;
		for( i = 0 ; i<m ; i++){
			bb[i] = maxof2(aa[i],0.0);
			fun_s += weight[i]* ( log(exp(-bb[i]) + exp(aa[i] - bb[i]) ) + bb[i] );
		}
		fun_s +=  ddot(n,s,1,s,1) *rsL2/2.0 ;

		// prob = 1./( 1 + exp(aa));
		for(i = 0; i < m ; i++)
			prob[i] = 1.0/ ( 1.0 + exp(aa[i]));

		// b = -diag(y.*weight) * (1-prob);
		dcopy(m,prob,1,b,1);
		daxpy(m,-1.0,oneVector,1,b,1);
		dtbmv('U','N','N',m,0,weighty,1,b,1);

		gc = ddot(n,b,1,oneVector,1);

		normalizedmv('T',&opt,A,b,g);

		// g = g + rsL2 * s;
		daxpy(n,rsL2,s,1,g,1);

		//xp = x; Axp= Ax;
		// cp = c;
		dcopy(n,x,1,xp,1);
		dcopy(m,Ax,1,Axp,1);

		cp = c;


		for( ;; ){
			// v = s - g/ gamma;
			dcopy(n,s,1,v,1);
			daxpy(n,-1.0/gamma,g,1,v,1);

			c = sc - gc/gamma;

			for(i = 0 ; i <n ; i++)
				vv[i] = maxof2(v[i],0.0);
			
			eplb(xx,&root,&steps,vv,n,z,lambda0);
			lambda0 = root;

			memset(x,0,sizeof(double)*n);
			for( i = 0 ; i <n; i++)
				if(v[i]>=0)
					x[i] = xx[i];

			//v = x  -s ;
			dcopy(n,x,1,v,1);
			daxpy(n,-1.0,s,1,v,1);

			normalizedmv('N',&opt,A,x,Ax);

			// aa = -diag(y) * (A*x + c);
			dcopy(m,Ax,1,aa,1);
			daxpy(m,c,oneVector,1,aa,1);
			dtbmv('U','N','N',m,0,y,1,aa,1);

			fun_x = 0;
			for( i=0; i<m ; i++){
				bb[i] = maxof2(aa[i],0);
				fun_x += weight[i]*( log(exp(-bb[i]) + exp(aa[i] - bb[i])) + bb[i]);
			}
			fun_x += rsL2 * ddot(n,x,1,x,1)/2.0;


			r_sum = (ddot(n,v,1,v,1) + (c-sc)*(c - sc))/2.0;
			l_sum = fun_x - fun_s - ddot(n,v,1,g,1) - (c-sc)*gc;

			if(r_sum <= 1.0e-20){
				bFlag = 1;
				break;
			}

			if( l_sum <= r_sum * gamma)
				break;
			else
				gamma = maxof2( 2*gamma, l_sum/r_sum);
		}

		tp = t;
		t = ( 1 + sqrt(4*t*t + 1.0))/2.0;

		//xxp = x - xp; 
		dcopy(n,x,1,xxp,1);
		daxpy(n,-1.0,xp,1,xxp,1);

		ccp = c - cp;

		fValp = fVal = funVal[iterStep] = fun_x;

		if(iterStep != 0)
			fValp = funVal[iterStep - 1];
		if(bFlag)
			break;

		if(terminationCondition(&opt,fVal,fValp,x,dnrm2(n,xxp,1),dnrm2(n,xp,1),iterStep))
			break;

	}

	funret.errorCode = 0;
	funret.c = c;
	funret.funVal = funVal;
	funret.totIter = iterStep;
	strcpy(funret.type,"LogisticR");
	funret.x = x;
	funret.ValueL = ValueL;
	return funret;
}